#!/usr/bin/python

"""
pyode-tutorial-3.py
This is tumbling box demo.

Written By:
    James Thomas
    Email: jim@houseoftechnology.org
    Web: http://mission-cognition.houseoftechnology.org/

Copyright 2009

This file is part of the PandaODElib distribution.

This library is free software; you can redistribute it and/or
modify it under the terms of EITHER:
  (1) The GNU Lesser General Public License as published by the Free
      Software Foundation; either version 2.1 of the License, or (at
      your option) any later version. The text of the GNU Lesser
      General Public License is included with this library in the
      file LICENSE.
  (2) The BSD-style license that is included with this library in
      the file LICENSE-BSD.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
LICENSE and LICENSE-BSD for more details. 

Note: Boxes that fly off at the beginning often come back eventually
"""

import random
import itertools

from pandac.PandaModules import Quat, Vec3, Mat4
import ode3d
import ode3d.lib
import pvis

class Box(ode3d.GDMBody):
    def __init__(self, World, Length, Height, Width):
        # initialize the parent class
        ode3d.GDMBody.__init__(self, World)
    
        shaft = ode3d.GDMElement().DefineBox(900, Length, Height, Width)
        shaft.SetCollisionBitfields(1,1) # Category = 0, collide with 0
        self.AddElement('box', shaft)

        #height = self._world.scene.height
        #width = self._world.scene.height
        #self.textDisplay = pvis.label(pos=(width-200,height-100-5,0), font='mono') # TODO: Need to fix screen sizes

class MyUserHandler(ode3d.lib.UserHandler):
    def __init__(self, App):
        ode3d.lib.UserHandler.__init__(self, App)
        self.boxes = []
        
    def UpdateSettings(self):
        ode3d.lib.UserHandler.UpdateSettings(self)

        for i, j in itertools.combinations(range(len(self.boxes)),2):
            b1 = self.boxes[i]
            b2 = self.boxes[j]
            
            # Get the masses
            m1 = b1.getMass().getMagnitude()
            m2 = b2.getMass().getMagnitude()
            
            # Compute the vectors from b1 to b2 and b2 to b1
            v12 = Vec3(b2.getPosition()) - Vec3(b1.getPosition())
            v21 = -v12 
            
            # Compute the gravitational pull between the boxes
            r2 = v12.lengthSquared()
            
            fudge = 1e6 # Speed up the sim by making inter-body gravity a million times higher
            F = fudge * 6.674e-11 * m1 * m2 / r2
            #print i, j, F
            
            # Apply the forces
            b1.addForce(v12 * F)
            b2.addForce(v21 * F)
             
class MyApp(ode3d.lib.OdeApp):
    def __init__(self):
        fps = 30 # Adjust FPS to suit your taste/hardware performance
        dt = 1/60.0 # the target rate for our sim time step
        gravity = (0,0,0) # Floating in space!
        
        # Now we can initialize the base class
        ode3d.lib.OdeApp.__init__(self, dt, fps, Gravity=gravity, UserHandlerClass=MyUserHandler)

        # Add arrows for the world axes and a sphere for the origin
        self.ax = pvis.arrow(q=pvis.xAxis, color=(0.5,0.2,0.2))
        self.ay = pvis.arrow(q=pvis.yAxis, color=(0.2,0.5,0.2))
        self.az = pvis.arrow(q=pvis.zAxis, color=(0.2,0.2,0.5))
        self.origin = pvis.sphere(radius=self.az.shaftwidth/2)

        numBoxes = 25 # 30 takes twice as long as 20, 60 is ~10x
        for index in range(numBoxes):
            # Create our tumbling box
            size = 2
            box = Box(self.world, size, size, size)
            self.AddObj('box%02d' % index, box)
            # Include the box in our user handler
            self._userHandler.boxes.append(box)
    
            # Setup a random error generator
            rand = random.Random()
            gauss = lambda x: rand.gauss(0.0, x)
            randf = random.random # shortcut  
    
            # Position the box in a random location within a radius 'groupSize' times its size
            groupSize = (size * 2 * numBoxes) ** 0.333333
            box.setPosition((gauss(groupSize),gauss(groupSize),gauss(groupSize)))
            
            # Apply a random initial orientation to our box
            q = Quat(box.getQuaternion())
            q *= Quat(1, randf(), randf(), randf()).normalize()
            box.setQuaternion(q)
    
            # Put an initial random spin on our box
            axis = Vec3(randf(), randf(), randf())
            speed = 5 * randf()
            box.setAngularVel(axis * speed)

        # Create an initial camera position
        pvis.base.disableMouse()
        pvis.base.camera.setPos(10 * groupSize,10 * groupSize,10 * groupSize)
        pvis.base.camera.lookAt(0, 0, 0) #@UndefinedVariable -- lookAt(), more Panda3d magic?
    
        # Turn mouse control back on
        mat=Mat4(pvis.base.camera.getMat()) #@UndefinedVariable -- getMat(), more Panda3d magic?
        mat.invertInPlace()
        pvis.base.mouseInterfaceNode.setMat(mat)
        pvis.base.enableMouse()

if __name__ == '__main__':
    app = MyApp()
    #app.Run(Static=True)
    app.Run(0)
